home *** CD-ROM | disk | FTP | other *** search
/ BCI NET 2 / BCI NET 2.iso / archives / programming / source / graphicgems4.lha / GemsIV / inv_fast.c < prev    next >
Encoding:
C/C++ Source or Header  |  1995-02-06  |  6.0 KB  |  179 lines
  1. /*
  2.  * ANSI C code from the article
  3.  * "Fast Inversion of Length- and Angle-Preserving Matrices"
  4.  * by Kevin Wu, Kevin.Wu@eng.sun.com
  5.  * in "Graphics Gems IV", Academic Press, 1994
  6.  *
  7.  * compile with "cc -DMAIN ..." to create a test program
  8.  */
  9.  
  10. #include "GraphicsGems.h"
  11. #include <stdio.h>
  12. /****
  13.  *
  14.  * angle_preserving_matrix4_inverse
  15.  *
  16.  * Computes the inverse of a 3-D angle-preserving matrix.
  17.  *
  18.  * This procedure treats the 4 by 4 angle-preserving matrix as a block
  19.  * matrix and calculates the inverse of one submatrix for a significant
  20.  * performance improvement over a general procedure that can invert any
  21.  * nonsingular matrix:
  22.  *        --          --      --               --
  23.  *        |           | -1      |   -2 T    -2  T    |
  24.  *        | A         C |      |  s    A     - s  A  C |
  25.  *    -1    |           |      |            |
  26.  *   M     =  |           |     =      |            |
  27.  *        | 0         1 |      |    0       1    |
  28.  *        |           |      |            |
  29.  *        --          --      --               --
  30.  * where      M is a 4 by 4 angle-preserving matrix,
  31.  *          A is the 3 by 3 upper-left submatrix of M,
  32.  *          C is the 3 by 1 upper-right submatrix of M.
  33.  *
  34.  * Input:
  35.  *   in      - 3-D angle-preserving matrix
  36.  *
  37.  * Output:
  38.  *   out  - inverse of 3-D angle-preserving matrix
  39.  *
  40.  * Returned value:
  41.  *   TRUE   if input matrix is nonsingular
  42.  *   FALSE  otherwise
  43.  *
  44.  ***/
  45.  
  46. boolean
  47. angle_preserving_matrix4_inverse (Matrix4 *in, Matrix4 *out)
  48. {
  49.     double  scale;
  50.  
  51.     /* Calculate the square of the isotropic scale factor */
  52.     scale = in->element[0][0] * in->element[0][0] +
  53.         in->element[0][1] * in->element[0][1] +
  54.         in->element[0][2] * in->element[0][2];
  55.  
  56.     /* Is the submatrix A singular? */
  57.     if (scale == 0.0) {
  58.  
  59.     /* Matrix M has no inverse */
  60.     fprintf (stderr, "angle_preserving_matrix4_inverse: singular matrix\n");
  61.     return FALSE;
  62.     }
  63.  
  64.     /* Calculate the inverse of the square of the isotropic scale factor */
  65.     scale = 1.0 / scale;
  66.  
  67.     /* Transpose and scale the 3 by 3 upper-left submatrix */
  68.     out->element[0][0] = scale * in->element[0][0];
  69.     out->element[1][0] = scale * in->element[0][1];
  70.     out->element[2][0] = scale * in->element[0][2];
  71.     out->element[0][1] = scale * in->element[1][0];
  72.     out->element[1][1] = scale * in->element[1][1];
  73.     out->element[2][1] = scale * in->element[1][2];
  74.     out->element[0][2] = scale * in->element[2][0];
  75.     out->element[1][2] = scale * in->element[2][1];
  76.     out->element[2][2] = scale * in->element[2][2];
  77.  
  78.     /* Calculate -(transpose(A) / s*s) C */
  79.     out->element[0][3] = - ( out->element[0][0] * in->element[0][3] +
  80.                  out->element[0][1] * in->element[1][3] +
  81.                  out->element[0][2] * in->element[2][3] );
  82.     out->element[1][3] = - ( out->element[1][0] * in->element[0][3] +
  83.                  out->element[1][1] * in->element[1][3] +
  84.                  out->element[1][2] * in->element[2][3] );
  85.     out->element[2][3] = - ( out->element[2][0] * in->element[0][3] +
  86.                  out->element[2][1] * in->element[1][3] +
  87.                  out->element[2][2] * in->element[2][3] );
  88.  
  89.     /* Fill in last row */
  90.     out->element[3][0] = out->element[3][1] = out->element[3][2] = 0.0;
  91.     out->element[3][3] = 1.0;
  92.  
  93.     return TRUE;
  94. }
  95.  
  96. #ifdef MAIN    /* test program for inverter */
  97.  
  98. /*
  99.  * Angle preserving matrix:
  100.  * M = S(-3.67, 1.85, 9.52) T(6.93, 6.93, 6.93) Ry(0.19) Rz(-1.32) Rx(0.87)
  101.  * where the angles are in radians.
  102.  */
  103. static double    m[4][4] =
  104.     {{  1.6889057579031668e+00,  5.2512935661266260e+00,
  105.        -4.1948078887213214e+00, -3.6699999999999999e+00 },
  106.      { -6.7131956438195779e+00,  1.1090087288191814e+00,
  107.        -1.3145356165599698e+00,  1.8500000000000001e+00 },
  108.      { -3.2481008100637232e-01,  4.3839383574315880e+00,
  109.         5.3572831630889803e+00,  9.5199999999999996e+00 },
  110.      {  0.0000000000000000e+00,  0.0000000000000000e+00,
  111.         0.0000000000000000e+00,  1.0000000000000000e+00 }};
  112.  
  113. main()
  114. {
  115.     Matrix4    in, out, prod;
  116.     int        i, j, k;
  117.  
  118.     for (i = 0; i < 4; i++) {
  119.     for (j = 0; j < 4; j++) {
  120.         in.element[i][j] = m[i][j];
  121.     }
  122.     }
  123.  
  124.     printf ("Original matrix:\n");
  125.     printf ("%13.6e %13.6e %13.6e %13.6e\n", in.element[0][0],
  126.         in.element[0][1], in.element[0][2], in.element[0][3]);
  127.     printf ("%13.6e %13.6e %13.6e %13.6e\n", in.element[1][0],
  128.         in.element[1][1], in.element[1][2], in.element[1][3]);
  129.     printf ("%13.6e %13.6e %13.6e %13.6e\n", in.element[2][0],
  130.         in.element[2][1], in.element[2][2], in.element[2][3]);
  131.     printf ("%13.6e %13.6e %13.6e %13.6e\n", in.element[3][0],
  132.         in.element[3][1], in.element[3][2], in.element[3][3]);
  133.  
  134.     /* Calculate inverse with utility */
  135.     angle_preserving_matrix4_inverse(&in, &out);
  136.  
  137.     printf ("\nCalculated inverse matrix:\n");
  138.     printf ("%13.6e %13.6e %13.6e %13.6e\n", out.element[0][0],
  139.         out.element[0][1], out.element[0][2], out.element[0][3]);
  140.     printf ("%13.6e %13.6e %13.6e %13.6e\n", out.element[1][0],
  141.         out.element[1][1], out.element[1][2], out.element[1][3]);
  142.     printf ("%13.6e %13.6e %13.6e %13.6e\n", out.element[2][0],
  143.         out.element[2][1], out.element[2][2], out.element[2][3]);
  144.     printf ("%13.6e %13.6e %13.6e %13.6e\n", out.element[3][0],
  145.         out.element[3][1], out.element[3][2], out.element[3][3]);
  146.  
  147.     /*
  148.      * Calculate the product of the original matrix and calculated inverse.
  149.      * The product should be the identity if the utility is correct.
  150.      */
  151.  
  152.     for (i = 0; i < 4; i++) {
  153.     for (j = 0; j < 4; j++) {
  154.         prod.element[i][j] = 0.0;
  155.     }
  156.     }
  157.  
  158.     for (i = 0; i < 4; i++) {
  159.     for (j = 0; j < 4; j++) {
  160.         for (k = 0; k < 4; k++) {
  161.         prod.element[i][j] += in.element[i][k] * out.element[k][j];
  162.         }
  163.     }
  164.     }
  165.  
  166.     printf ("\nProduct of original matrix and calculated inverse:\n");
  167.     printf ("%13.6e %13.6e %13.6e %13.6e\n", prod.element[0][0],
  168.         prod.element[0][1], prod.element[0][2], prod.element[0][3]);
  169.     printf ("%13.6e %13.6e %13.6e %13.6e\n", prod.element[1][0],
  170.         prod.element[1][1], prod.element[1][2], prod.element[1][3]);
  171.     printf ("%13.6e %13.6e %13.6e %13.6e\n", prod.element[2][0],
  172.         prod.element[2][1], prod.element[2][2], prod.element[2][3]);
  173.     printf ("%13.6e %13.6e %13.6e %13.6e\n", prod.element[3][0],
  174.         prod.element[3][1], prod.element[3][2], prod.element[3][3]);
  175.  
  176. }
  177.  
  178. #endif
  179.